#include "headfile.h"
/*-----------------------------------------------------------------------
        PB0左前   PA7左中     PA6左后     PA3右后     PA2右中     PA1右前
序号        1       2           3           4           5           6
以下为电机方向控制代码
------------------------------------------------------------------------*/
void Forward(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,x-10,0);
}

void TurnLeft(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,0,0);
}

void TurnRight(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,x-10,0);
}

void Left_PY(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,x,0);
}

void Right_PY(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,0,0);
}

void Diving(int x)
{
    ESC_Speed_config(5,x,1);
    ESC_Speed_config(2,x,1);
    ESC_Speed_config(4,120,0);      //maogugu
    ESC_Speed_config(3,120,0);      //maogugu
}

void Stop(void)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,0,0);
    ESC_Speed_config(5,0,1);
    ESC_Speed_config(2,0,1); 
}

void Rapidly_Right(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,2.2*x,0);
    ESC_Speed_config(3,0,0);    
}

void Rapidly_Left(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,2.2*x-10,0);    
}



/*-----------------------------------------------------------------------
红外传感器定深代码
(每个控制周期进入)
------------------------------------------------------------------------*/
void Dingshen(void)
{
    // if(HW1 && HW2 && HW3 && HW4 && HW5)
    if(Res[2] == 'Y')
        Diving(180);     //下潜（修改值）
    else
        Diving(0);    
}

/*-----------------------------------------------------------------------
红外传感器平移代码
（每隔n个周期进入一次）
------------------------------------------------------------------------*/
void Pingyi(void)
{
    if(HW1 + HW2 >= HW4 + HW5)
        Right_PY(150);  //右平移（修改值） 
    else if (HW1 + HW2 == HW4 + HW5)
        Forward(150);  //正常运行（修改值）  
    else if (HW1 + HW2 <= HW4 + HW5) 
        Left_PY(150);   //左平移 （修改值）

}

/*-----------------------------------------------------------------------
串口接收控制代码
目前仅设计了串口接收信息并控制姿态
------------------------------------------------------------------------*/
void USART_Data_Analysis(void)
{
    switch(Res[0])
    {
        case 'F':
            TurnLeft(50);   //左转（可修改））
            break;

        case 'D':
            TurnLeft(60);   //左转（可修改）
            break;

        case 'S':
            TurnLeft(70);   //左转（可修改）
            break;

        case 'A':
            TurnLeft(90);  //左转（可修改）
            break;

        case 'H':
            TurnRight(50);  //右转（可修改）
            break;

        case 'J':
            TurnRight(60);   //右转（可修改）
            break;

        case 'K':
            TurnRight(70);  //右转（可修改）
            break;

        case 'L':
            TurnRight(90);  //右转（可修改）
            break;

        case 'G':
            Forward(70);    //前进（可修改）
            break;

        case 'P':
            Stop();  
            break;
        default:
            break;
    }

	switch(Res[1])  
	{
		case 'C':
			reco_c++;
			reco_flag++;					
				break;
		case 'Q':
			reco_s++;
			reco_flag++;
				break;
		default:
			reco_flag++;
				break;            
	}
}

void Init_Config(void)
{
	LED_Init_Config();
	EXTI_Init_Config();
	USART_Config();
	ESC_Strat_Init(); //最后初始化（内含定时器初始化）
}


/*
以下采集代码与控制代码均在中断中使用
*/
void Data_Collect(void)
{
		EXTI_ReadBit();
}

void Vision_Recognition(void)
{
	while(reco_flag == 50)
	{
		if(reco_s + reco_c < 1)
		{
			LED1(0);LED2(0);LED3(0);
			reco_s = 0;
			reco_c = 0;
			reco_flag = 0;
			break;
		}
		else if (reco_s == reco_c)
		{
			LED1(0);LED2(0);LED3(0);
			reco_s = 0;
			reco_c = 0;
			reco_flag = 0;
			break;		
		}
		else
		{
			if(reco_s == Max(reco_s,reco_c))
			{
				LED1(1);LED2(0);LED3(0);
				led_flag = 1;
			}
			else
			{
				LED1(0);LED2(1);LED3(0);
				led_flag = 1;
			}
			reco_s = 0;
			reco_c = 0;
			reco_flag = 0;						
			break;
		}
	}
}

void control_Config(void)
{
	Data_Collect();
    USART_Data_Analysis();
	// Dingshen();
	
	if(led_flag == 0)
		Vision_Recognition();
	else if(led_flag == 1)
	{
		led_delay++;
		if(led_delay == 200)
		led_flag = 0;
	}
}



